/*
 * Other.c
 *
 *  Created on: Jun 10, 2024
 *      Author: Liangmiyi
 */
#include <MY_INC/PHY/PHY_PLL.h>
static inline void PLL_Reset_Inline(PLL_Typedef *PLL);
void               PLL_Init(PLL_Typedef *PLL, float Kp, float Ki, float Feedforward, float ts)
{
    if (PLL == NULL)
        return;

    PLL->Coeff_B0 = (2 * Kp + Ki * ts) / 2.0f; // 比例增益
    PLL->Coeff_B1 = -(2 * Kp - Ki * ts) / 2.0f;
    PLL->ts_half = ts * 0.5f;       // 预计算 0.5 * ts
    PLL->Feedforward = Feedforward; // 初始化前馈值和系数
    PLL_Reset_Inline(PLL);
}

void PLL_Reset(PLL_Typedef *PLL)
{
    if (PLL == NULL)
        return;
    PLL_Reset_Inline(PLL); // 重置 PLL 结构体
}
static inline void PLL_Reset_Inline(PLL_Typedef *PLL)
{
    PLL->Input = 0.0f;         // 初始化输入
    PLL->Yn = 0.0f;            // 上一次输出
    PLL->Xn_1 = 0.0f;          // 上一次输入
    PLL->Integral_Xn_1 = 0.0f; // 初始化积分变量
    PLL->Integral_Yn = 0.0f;
    PLL->W = 0.0f; // 初始化频率和相位
    PLL->Theta = 0.0f;
}